#pragma once
#include "vex.h"

class pid_ctrl {
 public:
  bool print_detail = true;

 public:
  float aim = 0;
  float blind_area = 0;

 private:
  float kp;
  float P_put;

  float ki;
  float II_start;
  float max_I_put;
  float max_II;

  float I_put = 0;
  float II = 0;

  float kd;
  float DD;
  float D_put;
  float D_tolerance;

  float max_pwr = 100;

  float puts = 0;
  float err_now = 0;
  float err_last = 0;

  float val_now = 0;
  int begin_dir = 74;

  timer t_run;
  int settle_time = 150;
  bool settled = false;

 public:
  pid_ctrl() : pid_ctrl(0, 0, 0, 0, 0, 0, 0, 0) {}
  pid_ctrl(float kp, float ki, float kd, float max_pwr, float blind_area = 2,
           float max_I_put = 10, float II_start = 0, float aim = 0);

  // 老版本的函数参数顺序
  static pid_ctrl get_instance_old_version(float kp, float ki, float kd,
                                           float max_I_put, float aim,
                                           float blind_area, float II_start,
                                           float max_pwr) {
    return pid_ctrl(kp, ki, kd, max_pwr, blind_area, max_I_put, II_start, aim);
  }

  // 构造一个pid控制器,使用函数方便vscode显示形参名字，与直接使用构造函数不会显示形参名
  static pid_ctrl get_instance(float kp, float ki, float kd, float max_pwr,
                               float blind_area = 2, float max_I_put = 10,
                               float II_start = 0, float aim = 0) {
    return pid_ctrl(kp, ki, kd, max_pwr, blind_area, max_I_put, II_start, aim);
  }

  void set_kp(float kp) { this->kp = kp; }
  void set_ki(float ki) { this->ki = ki; }
  void set_kd(float kd) { this->kd = kd; }
  void set_max_I_put(float max_I_put) { this->max_I_put = max_I_put; }
  void set_aim(float aim) { this->aim = aim; }
  void set_blind_area(float blind_area) { this->blind_area = blind_area; }
  void set_II_start(float II_start) { this->II_start = II_start; }
  void set_max_pwr(float max_pwr) { this->max_pwr = max_pwr; }

  void reset_II() { II = 0; }

  float pid_calc();
  bool is_overshoot() { return (begin_dir != sgn(err_now)); }
  bool is_err_in_blind() { return fabs(err_now) < blind_area; }
  bool is_settled() { return settled; }

  float get_err() { return err_now; }
  float get_II() { return II; }
  float get_DD() { return DD; }

  float get_P_put() { return P_put; }
  float get_I_put() { return I_put; }
  float get_D_put() { return D_put; }
  float get_all_put() { return puts; }

  float pid_output(float value_now);

  float pid_output_need_value(float aim_now, float value_now);

  float pid_output_need_err(float err_src);

  void set_print_detail(bool p) { print_detail = p; }
  void print();
};
